/******************************************************************************
*  Nano-RK, a real-time operating system for sensor networks.
*  Copyright (C) 2007, Real-Time and Multimedia Lab, Carnegie Mellon University
*  All rights reserved.
*
*  This is the Open Source Version of Nano-RK included as part of a Dual
*  Licensing Model. If you are unsure which license to use please refer to:
*  http://www.nanork.org/nano-RK/wiki/Licensing
*
*  This program is free software: you can redistribute it and/or modify
*  it under the terms of the GNU General Public License as published by
*  the Free Software Foundation, version 2.0 of the License.
*
*  This program is distributed in the hope that it will be useful,
*  but WITHOUT ANY WARRANTY; without even the implied warranty of
*  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
*  GNU General Public License for more details.
*
*  You should have received a copy of the GNU General Public License
*  along with this program.  If not, see <http://www.gnu.org/licenses/>.
*
*******************************************************************************/

//#include <nrk.h>
#include <nrk_defs.h>
#include <include.h>
#include <ulib.h>
#include <stdio.h>
#include <hal.h>
#include <nrk_error.h>
#include <nrk_timer.h>
#include <nrk_stack_check.h>
#include <nrk_stats.h>
#include <avr/wdt.h>


NRK_STK Stack1[NRK_APP_STACKSIZE];
nrk_task_type TaskOne;
void Task1(void);

NRK_STK Stack2[NRK_APP_STACKSIZE];
nrk_task_type TaskTwo;
void Task2 (void);

NRK_STK Stack3[NRK_APP_STACKSIZE];
nrk_task_type TaskThree;
void Task3 (void);


NRK_STK Stack4[NRK_APP_STACKSIZE];
nrk_task_type TaskFour;
void Task4 (void);

void nrk_create_taskset();
uint8_t kill_stack(uint8_t val);

void now()
{
	nrk_time_t now;
	nrk_time_get(&now);
	//printf("Now: %d.%0.9d ", now.secs, now.nano_secs);
	printf("Now: %d.%d ", now.secs, now.nano_secs);
}

nrk_sem_t *sem_a;
nrk_sem_t *sem_b;

int
main ()
{
	nrk_setup_ports();
	nrk_setup_uart(UART_BAUDRATE_115K2);

	nrk_init();
	wdt_disable();

	nrk_led_clr(ORANGE_LED);
	nrk_led_clr(BLUE_LED);
	nrk_led_clr(GREEN_LED);
	nrk_led_clr(RED_LED);

	nrk_time_set(0,0);
	nrk_create_taskset ();

	// Create semaphores - nrk_sem_create(count, ceiling)
	sem_a = nrk_sem_create(1, 10);
	if(sem_a == NULL)
		nrk_kprintf( PSTR("Error creating sem a\r\n" ));
	
	// sem_b = nrk_sem_create(1,4);
	// if(sem_b == NULL)
		// nrk_kprintf( PSTR("Error creating sem b\r\n" ));
	
	nrk_start();

	return 0;
}

void Task1()
{
	nrk_time_t t;
	uint16_t cnt;
	cnt=0;
	
	while(1)
	{
		now(); printf( "Task1 start cnt=%u\r\n",cnt );
		uint32_t i = 1000;
		
		nrk_sem_pend(sem_a);
		now(); printf( "Task1 has sem_a\r\n" );
		while(i--)
		{
			nrk_kprintf(PSTR("1"));
		}
		now(); printf( "Task1 released sem_a\r\n" );
		nrk_sem_post(sem_a);
		
		now(); printf( "Task1 end cnt=%u\r\n",cnt );
		nrk_wait_until_next_period();
		cnt++;
	}
}

void Task2()
{
	nrk_time_t t;
	uint16_t cnt;
	cnt=0;
	
	while(1)
	{
		now(); printf( "Task2 start cnt=%u\r\n",cnt );
		
		nrk_sem_pend(sem_a);
		now(); printf( "Task2 has sem_a\r\n" );
		
		now(); printf( "Task2 released sem_a\r\n" );
		nrk_sem_post(sem_a);
		
		now(); printf( "Task2 end cnt=%u\r\n",cnt );
		nrk_wait_until_next_period();
		cnt++;
	}
}

void Task3()
{
	nrk_time_t t;
	uint16_t cnt;
	cnt=0;
	
	while(1)
	{
		printf( "Task3 start cnt=%u\r\n",cnt );
		
		printf( "Task3 end cnt=%u\r\n",cnt );
		nrk_wait_until_next_period();
		cnt++;
	}
}

void Task4()
{
	nrk_time_t t;
	uint16_t cnt;
	cnt=0;
	
	while(1)
	{
		printf( "Task4 start cnt=%u\r\n",cnt );
		
		printf( "Task4 end cnt=%u\r\n",cnt );
		nrk_wait_until_next_period();
		cnt++;
	}
}

void
nrk_create_taskset()
{
	nrk_task_set_entry_function( &TaskOne, Task1);
	nrk_task_set_stk( &TaskOne, Stack1, NRK_APP_STACKSIZE);
	TaskOne.prio = 1;
	TaskOne.FirstActivation = TRUE;
	TaskOne.Type = BASIC_TASK;
	TaskOne.SchType = PREEMPTIVE;
	TaskOne.period.secs = 60;
	TaskOne.period.nano_secs = 0*NANOS_PER_MS;
	TaskOne.cpu_reserve.secs = 0;
	TaskOne.cpu_reserve.nano_secs = 0*NANOS_PER_MS;
	TaskOne.offset.secs = 0;
	TaskOne.offset.nano_secs= 0;
	TaskOne.sem_list[0] = sem_a;
	TaskOne.sem_count = 1;
	nrk_activate_task (&TaskOne);

	nrk_task_set_entry_function( &TaskTwo, Task2);
	nrk_task_set_stk( &TaskTwo, Stack2, NRK_APP_STACKSIZE);
	TaskTwo.prio = 2;
	TaskTwo.FirstActivation = TRUE;
	TaskTwo.Type = BASIC_TASK;
	TaskTwo.SchType = PREEMPTIVE;
	TaskTwo.period.secs = 10;
	TaskTwo.period.nano_secs = 0*NANOS_PER_MS;
	TaskTwo.cpu_reserve.secs = 0;
	TaskTwo.cpu_reserve.nano_secs = 0*NANOS_PER_MS;
	TaskTwo.offset.secs = 0;
	TaskTwo.offset.nano_secs= 0;
	nrk_activate_task (&TaskTwo);


	nrk_task_set_entry_function( &TaskThree, Task3);
	nrk_task_set_stk( &TaskThree, Stack3, NRK_APP_STACKSIZE);
	TaskThree.prio = 2;
	TaskThree.FirstActivation = TRUE;
	TaskThree.Type = BASIC_TASK;
	TaskThree.SchType = PREEMPTIVE;
	TaskThree.period.secs = 5;
	TaskThree.period.nano_secs = 0;
	TaskThree.cpu_reserve.secs = 1;
	TaskThree.cpu_reserve.nano_secs = 0*NANOS_PER_MS;
	TaskThree.offset.secs = 0;
	TaskThree.offset.nano_secs= 0;
	//nrk_activate_task (&TaskThree);


	nrk_task_set_entry_function( &TaskFour, Task4);
	nrk_task_set_stk( &TaskFour, Stack4, NRK_APP_STACKSIZE);
	TaskFour.prio = 1;
	TaskFour.FirstActivation = TRUE;
	TaskFour.Type = BASIC_TASK;
	TaskFour.SchType = PREEMPTIVE;
	TaskFour.period.secs = 10;
	TaskFour.period.nano_secs = 0;
	TaskFour.cpu_reserve.secs = 1;
	TaskFour.cpu_reserve.nano_secs = 0*NANOS_PER_MS;
	TaskFour.offset.secs = 0;
	TaskFour.offset.nano_secs= 0;
	//nrk_activate_task (&TaskFour);

}

uint8_t kill_stack(uint8_t val)
{
	char bad_memory[10];
	uint8_t i;
	for(i=0; i<10; i++ ) 
		bad_memory[i]=i;
	for(i=0; i<10; i++ )
		printf( "%d ", bad_memory[i]);
	printf( "Die Stack %d\r\n",val );
	if(val>1)
		kill_stack(val-1);
	return 0;
}


